#include "os_sys.h"
#include "motor_driver.h"
#include "motor_bsp.h"

struct motor_task_s
{
    SysTick_t motor_time;
	u32 test[3][2];
};

struct motor_task_s motor_task;





void motor_task_test(E_MOTOR_PWM index,E_ROTATION dir)
{
	if((MOTOR_FRONT_BACK == index) || (MOTOR_LEFT_RIGTH==index))
		motor_pwm_stop_immediately(index);//motor_pwm_stop_decel
	else
		motor_pwm_stop_immediately(index);
	motor_task.test[index][dir]++;
}



u32 motor_test=0;
u32 motor_buff[4]={500,250,50000};
s32 motor_coor[MOTOR_PWM_MAX]={0};
static void motor_main_run(void)
{
	if(0 == systick_get_difference_assign(&motor_task.motor_time,Tnms(200)))
		return ;

	motor_coor[MOTOR_ANGLE]=motor_pwm_dma_real_coordinate_get(MOTOR_ANGLE);
	motor_coor[MOTOR_FRONT_BACK]=motor_pwm_dma_real_coordinate_get(MOTOR_FRONT_BACK);
	motor_coor[MOTOR_LEFT_RIGTH]=motor_pwm_dma_real_coordinate_get(MOTOR_LEFT_RIGTH);
	switch(motor_test)
	{
		case 20:
			motor_pwm_curve_start(MOTOR_FRONT,motor_buff[0], motor_buff[2]);
			break;
		case 21:
			motor_pwm_curve_start(MOTOR_BACK,motor_buff[0], motor_buff[2]);
			break;
		case 22:
			motor_pwm_curve_start(MOTOR_LEFT,motor_buff[0], motor_buff[2]);
			break;
		case 23:
			motor_pwm_curve_start(MOTOR_RIGHT,motor_buff[0], motor_buff[2]);
			break;
		case 24:
			motor_pwm_square_start(MOTOR_ANGLE_DEC, motor_buff[0], motor_buff[2]);
		break;
		case 25:
			motor_pwm_square_start(MOTOR_ANGLE_INC, motor_buff[0], motor_buff[2]);
		break;
		default:
		break;
	}

	motor_test =0;
}
static void motor_peripheral_sleep(void)
{

}
static void motor_message_process(E_DATAID DatID, const u32 data)
{
	switch(DatID)
	{
		default:break;
	}
}
static void motor_variable_init(void)
{

	motor_limit_callback_set(MOTOR_FRONT,motor_task_test);
	motor_limit_callback_set(MOTOR_BACK,motor_task_test);
	motor_limit_callback_set(MOTOR_LEFT,motor_task_test);
	motor_limit_callback_set(MOTOR_RIGHT,motor_task_test);
	motor_limit_callback_set(MOTOR_ANGLE_DEC,motor_task_test);
	motor_limit_callback_set(MOTOR_ANGLE_INC,motor_task_test);

	motor_pwm_dma_coordinate_init(MOTOR_FRONT_BACK);
	motor_pwm_dma_coordinate_init(MOTOR_LEFT_RIGTH);
	motor_pwm_dma_coordinate_init(MOTOR_ANGLE);

	motor_bsp_init();
}


//CONSTRUCTOR_DEFAUTL()
void motor_task_create(void)
{
	struct task_s task = TASK_DEFAULT_INIT();

	task.ID 		= TASKID_MOTOR;
	task.state		= TASK_STAT_FREE;
	task.TaskTime	= 0;
	task.run 		= motor_main_run;
	task.sleep_init = motor_peripheral_sleep;
	task.msg_process= motor_message_process;
	task.task_tick 	= NULL;
	task.is_used 	= 1;
	task.task_init =   motor_variable_init;
	sys_task_add(&task);
}

